#include "defaults.h"
#include "edge.h"
#include "triangle.h"
#include "utils.h"
#include "nurbs.h"
#include "math/numerics.h"

STriangle::STriangle(Node* v0, Node* v1, Node* v2, Attribute& attribute, size_t id) : ElementImplementation<STriangle>(attribute, id) {
	vertex[0] = v0;
	vertex[1] = v1;
	vertex[2] = v2;
	edge[0] = edge[1] = edge[2] = 0;
	neighbor = 0;
}

Mat4x4n STriangle::Transform(const std::string& attribute) const {
	const Vec3n& a = vertex[0]->Get<Vec3n>(attribute);
	const Vec3n& b = vertex[1]->Get<Vec3n>(attribute);
	const Vec3n& c = vertex[2]->Get<Vec3n>(attribute);
	
	Vec3n u = b - a;
	Vec3n v = c - a;
	Vec3n w = cross(u, v);
	Vec3n z = math::V3n(0.0, 0.0, 1.0);
	normalize(w);
	Vec3n x = cross(w, z);
	normalize(x);
	nfloat theta = (180.0 * acos(dot(w, z))) / M_PI;
	Mat4x4n T;
	T.Rotate(-theta, x); 
	T.Translate(-a[0], -a[1], -a[2]);
	return T;
}

Mat4x4n STriangle::Transform2(const std::string& attribute) const {
	const Vec3n& a = vertex[0]->Get<Vec3n>(attribute);
	const Vec3n& b = vertex[1]->Get<Vec3n>(attribute);
	const Vec3n& c = vertex[2]->Get<Vec3n>(attribute);
	
	Mat4x4n M3 = Transform(attribute);
	
	Vec4n temp = M3 * math::V4n(a, 1.0);
	const Vec3n a2 = math::V3n(temp[0], temp[1], temp[2]);
	
	temp = M3 * math::V4n(b, 1.0);
	const Vec3n b2 = math::V3n(temp[0], temp[1], temp[2]);
	
	temp = M3 * math::V4n(c, 1.0);
	const Vec3n c2 = math::V3n(temp[0], temp[1], temp[2]);
	M3.Inverse();
	
	Mat4x4n M4(b2[0] - a2[0], c2[0] - b2[0], a2[0], 0.0,
			   b2[1] - a2[1], c2[1] - b2[1], a2[1], 0.0,
			   0.0, 0.0, 1.0, 0.0,
			   0.0, 0.0, 0.0, 1.0);
	
	return M3 * M4;
}

void STriangle::ComputeBaryCentric(const Vec3n& P, nfloat& a, nfloat& b, nfloat& c, const std::string& attribute) const {
	Mat4x4n M2 = Transform2(attribute);
	M2.Inverse();

	Vec4n u = M2 * math::V4n(P, 1.0);

	const nfloat& x = u[0];
	const nfloat& y = u[1];

	a = 1.0 - x;
	b = x - y;
	c = y;
}
	
Vec3n STriangle::Normal(const std::string& attribute) const {
	const Vec3n& a = vertex[0]->Get<Vec3n>(attribute);
	const Vec3n& b = vertex[1]->Get<Vec3n>(attribute);
	const Vec3n& c = vertex[2]->Get<Vec3n>(attribute);
	Vec3n normal = cross(b - a, c - a);
	normalize(normal);
	return normal;
}

nfloat STriangle::Area(const std::string& attribute) const {
	const Vec3n& p0 = vertex[0]->Get<Vec3n>(attribute);
	const Vec3n& p1 = vertex[1]->Get<Vec3n>(attribute);
	const Vec3n& p2 = vertex[2]->Get<Vec3n>(attribute);
		
	const Vec3n pq = p1 - p0;
	const Vec3n pr = p2 - p0;
	
	const Vec3n pqxpr = cross(pq, pr);

	return 0.5 * length(pqxpr);
}

Vec3n STriangle::Center(const std::string& attribute) const {
	return (1.0 / 3.0) * (vertex[0]->Get<Vec3n>(attribute) + vertex[1]->Get<Vec3n>(attribute) + vertex[2]->Get<Vec3n>(attribute));
}

Matrix STriangle::DetermineCoefficients3(const Vec2n& p0, const Vec2n& p1, const Vec2n& p2) const {
	Matrix A(3, 3);
	A(0, 0) = p0[0];
	A(1, 0) = p0[1];
	A(2, 0) = 1.0;
	A(0, 1) = p1[0];
	A(1, 1) = p1[1];
	A(2, 1) = 1.0;
	A(0, 2) = p2[0];
	A(1, 2) = p2[1];
	A(2, 2) = 1.0;
	
	if(std::abs(math::Det(A)) < 0.0000000000001) {
		return Matrix(0, 0);
	}
	
	math::Invert(A);
	
	return A;
}

bool STriangle::Init(const std::string& attribute) {
	
	return true;
}

Matrix STriangle::Stiffness(const std::string& attribute) const {
	const Vec3n& a = vertex[0]->Get<Vec3n>(attribute);
	const Vec3n& b = vertex[1]->Get<Vec3n>(attribute);
	const Vec3n& c = vertex[2]->Get<Vec3n>(attribute);
	
	const Mat4x4n T = Transform(attribute);
	
	const Vec4n at = T * math::V4n(a, 1.0);
	const Vec4n bt = T * math::V4n(b, 1.0);
	const Vec4n ct = T * math::V4n(c, 1.0);
	
	const Vec2n a2 = math::V2n(at[0], at[1]);
	const Vec2n b2 = math::V2n(bt[0], bt[1]);
	const Vec2n c2 = math::V2n(ct[0], ct[1]);
    Matrix beta = DetermineCoefficients3(a2, b2, c2);
	Matrix K(3, 3);

    const nfloat J = 0.5 * length(cross(b - a, c - a));
	for(size_t i = 0; i < 3; i++) {
        const Vec2n gradi = math::V2n(beta(i, 0), beta(i, 1));
		for(size_t j = 0; j < 3; j++) {
            const Vec2n gradj = math::V2n(beta(j, 0), beta(j, 1));
			K(i, j) = dot(gradi, gradj);
		}
	}

	return -J * K;
}

nfloat STriangle::Mass(const size_t i, const size_t j, const Matrix& beta, const Mat4x4n& M) const {
	const nfloat scale = 1.0;
	static nfloat g[16][3] = {//u, v, w
		{ 0.33333333333333331, 0.33333333333333331, scale * 0.1443156076777862 },
		{ 0.081414823414554124, 0.4592925882927229, scale * 0.095091634267284966 },
		{ 0.4592925882927229, 0.4592925882927229, scale * 0.095091634267284966 },
		{ 0.4592925882927229, 0.081414823414554124, scale * 0.095091634267284966 },
		{ 0.89890554336593786, 0.050547228317031033, scale * 0.032458497623198135 },
		{ 0.050547228317031033, 0.050547228317031033, scale * 0.032458497623198135 },
		{ 0.050547228317031033, 0.89890554336593786, scale * 0.032458497623198135 },
		{ 0.65886138449647969, 0.1705693077517601, scale * 0.1032173705347184 },
		{ 0.1705693077517601, 0.1705693077517601, scale * 0.1032173705347184 },
		{ 0.1705693077517601, 0.65886138449647969, scale * 0.1032173705347184 },
		{ 0.0083947774099572114, 0.72849239295540413, scale * 0.027230314174434864 },
		{ 0.0083947774099572114, 0.26311282963463872, scale * 0.027230314174434864 },
		{ 0.72849239295540413, 0.26311282963463872, scale * 0.027230314174434864 },
		{ 0.72849239295540413, 0.0083947774099572114, scale * 0.027230314174434864 },
		{ 0.26311282963463872, 0.0083947774099572114, scale * 0.027230314174434864 },
		{ 0.26311282963463872, 0.72849239295540413, scale * 0.027230314174434864 }
	};
	
	nfloat m = 0.0;
	for(size_t k = 0; k < 16; k++) {
		const nfloat& u = g[k][0];
		const nfloat& v = g[k][1];
		const nfloat& w = g[k][2];
		Vec4n p = M * math::V4n(1.0 - u, v, 0.0, 1.0);
		const nfloat x = p[0];
		const nfloat y = p[1];

		const nfloat bi = beta(i, 0) * x + beta(i, 1) * y + beta(i, 2);
		const nfloat bj = beta(j, 0) * x + beta(j, 1) * y + beta(j, 2);

		m += w * bi * bj;
	}
	return m;
}

Matrix STriangle::Mass(const std::string& attribute, const bool lumped) const {
	Matrix M = ZeroMatrix(3, 3);
	if(lumped) {
		nfloat area = Area(attribute) / 3.0;
		for(size_t i = 0; i < 3; i++)
			M(i, i) = area;
		return M;
	}

	const Vec3n& a = vertex[0]->Get<Vec3n>(attribute);
	const Vec3n& b = vertex[1]->Get<Vec3n>(attribute);
	const Vec3n& c = vertex[2]->Get<Vec3n>(attribute);

	Mat4x4n M3 = Transform(attribute);

	const Vec4n at = M3 * math::V4n(a, 1.0);
	const Vec4n bt = M3 * math::V4n(b, 1.0);
	const Vec4n ct = M3 * math::V4n(c, 1.0);

	const Vec2n a2 = math::V2n(at[0], at[1]);
	const Vec2n b2 = math::V2n(bt[0], bt[1]);
	const Vec2n c2 = math::V2n(ct[0], ct[1]);

	const Matrix beta = DetermineCoefficients3(a2, b2, c2);

	Mat4x4n M4(b2[0] - a2[0], c2[0] - b2[0], a2[0], 0.0,
			   b2[1] - a2[1], c2[1] - b2[1], a2[1], 0.0,
			   0.0, 0.0, 1.0, 0.0,
			   0.0, 0.0, 0.0, 1.0);

	Mat4x4n M1 = M4;

	const nfloat J = 0.5 * length(cross(b - a, c - a));
	for(size_t i = 0; i < 3; i++) {
		for(size_t j = i; j < 3; j++) {
			const nfloat mij = J * Mass(i, j, beta, M1);
			M(i, j) = mij; 
			M(j, i) = mij; 
		}
	}

	return M;
}

bool IsAdjacent(STriangle* t1, STriangle* t2) {
	for(size_t i = 0; i < 3; i++) {
		if(t1->edge[i]->neighbor && t1->edge[i]->neighbor->triangle == t2)
			return true;
	}
	return false;
}
